#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/SceneObject.h>

#include "EnvironmentalContactSensorVisualisation.h"

using namespace MMM;

EnvironmentalContactSensorVisualisation::EnvironmentalContactSensorVisualisation(EnvironmentalContactSensorPtr sensor, VirtualRobot::RobotPtr robot, VirtualRobot::CoinVisualizationPtr visualization, SoSeparator* sceneSep) :
    SensorVisualisation(sceneSep),
    sensor(sensor),
    robot(robot),
    visualization(visualization)
{
}

std::string EnvironmentalContactSensorVisualisation::getType() {
    return sensor->getType();
}

int EnvironmentalContactSensorVisualisation::getPriority() {
    return sensor->getPriority();
}

void EnvironmentalContactSensorVisualisation::update(float timestep, float delta) {
    update(timestep);
}

void EnvironmentalContactSensorVisualisation::update(float timestep) {
    EnvironmentalContactSensorMeasurementPtr measurement = sensor->getDerivedMeasurement(timestep);
    if (!measurement) return;

    auto mContacts = measurement->getEnvironmentalContact();

    for (auto it = contacts.begin(); it != contacts.end(); it++) {
        if (mContacts.find(*it) == mContacts.end()) {
            this->robot->getRobotNode(*it)->highlight(visualization, false);
            contacts.erase(it);
        }
    }
    for (const auto &contact : mContacts) {
        if (contacts.find(contact.first) == contacts.end()) {
            this->robot->getRobotNode(contact.first)->highlight(visualization, true);
            contacts.insert(contact.first);
        }
    }

}

void EnvironmentalContactSensorVisualisation::displayVisualisation(bool display) {
    SensorVisualisation::displayVisualisation(display);
    if (display) update(lastTimestep, lastDelta);
    else {
        this->robot->highlight(visualization, false);
        this->contacts.clear();
    }
}
